﻿/*
 * drive.c
 *
 * Created: 04.04.13 0:16:58
 *  Author: ЖЕНЯ
 */ 

#include <avr/io.h>
#include "drive.h"
#include "config.h"


//////////////////
void InitDrive(uint8_t id)
{
	// fast PWM, mode 14, TOP-ICR, prescaler=8
	TCNT1 = 0x0000;	
	// COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	TCCR1A |= (1 << WGM11);	
	// ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10
	TCCR1B |= (1 << WGM13) | (1<<WGM12) | (1 << CS11);	
	ICR1 = 0x80;	// freq 15.5 kHz
	OCR1A = 0x0;
	
	// fast PWM, mode 14, TOP-ICR, prescaler=8
	TCNT5 = 0x0000;
	// COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	TCCR5A |= (1 << WGM11);
	// ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10
	TCCR5B |= (1 << WGM13) | (1<<WGM12) | (1 << CS11);
	ICR5 = 0x80;	// freq 15.5 kHz
	OCR5A = 0x0;
	
	// fast PWM, mode 14, TOP-ICR, prescaler=8
	TCNT3 = 0x0000;
	// COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	TCCR3A |= (1 << WGM11);
	// ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10
	TCCR3B |= (1 << WGM13) | (1<<WGM12) | (1 << CS11);
	ICR3 = 0x80;	// freq 15.5 kHz
	OCR3A = 0x0;
		
	// fast PWM, mode 14, TOP-ICR, prescaler=8
	TCNT4 = 0x0000;
	// COM1A1 COM1A0 COM1B1 COM1B0 COM1C1 COM1C0 WGM11 WGM10
	TCCR4A |= (1 << WGM11);
	// ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10
	TCCR4B |= (1 << WGM13) | (1<<WGM12) | (1 << CS11);
	ICR4 = 0x80;	// freq 15.5 kHz
	OCR4A = 0x0;
	
	switch(id){
		case DRIVE0:
				M0ADDR	|= (1<<M0AHI)|(1<<M0ALO);
				M0BDDR	|= (1<<M0BHI)|(1<<M0BLO);
				M0RSTDDR |= (1<<M0RST);
			break;
		case DRIVE1:
				M1ADDR	|= (1<<M1AHI)|(1<<M1ALO);
				M1BDDR	|= (1<<M1BHI)|(1<<M1BLO);
				M1RSTDDR |= (1<<M1RST);
			break;
		case ALL_DRIVE:
				M0ADDR	|= (1<<M0AHI)|(1<<M0ALO);
				M0BDDR	|= (1<<M0BHI)|(1<<M0BLO);
				M0RSTDDR |= (1<<M0RST);
				
				M1ADDR	|= (1<<M1AHI)|(1<<M1ALO);
				M1BDDR	|= (1<<M1BHI)|(1<<M1BLO);
				M1RSTDDR |= (1<<M1RST);
			break;
		default: break;
	}
	DriveRst(id);
}

void UpdateDrive(uint8_t idx, uint8_t value)
{

	
	if(value == 0x0) DriveRst(idx);
	else
	switch(idx){
		case DRIVE0:
				M0RSTPORT |= (1<<M0RST);
				M0APORT |= (1<<M0AHI);
				M0BPORT |= (1<<M0BHI);
				
				if(CHECKBIT(value,7)) {
					if(curConfig[0].drvDir){		// test for CW/CCW
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR5A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
					
						ICR1 = 0x80;
						OCR1A = (value & 0x7f);	// low 7 bit
						TCCR1A |= (1<<COM1A1);	
					} else {
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR1A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
						ICR5 = 0x80;
						OCR5A = (value & 0x7f);	// low 7 bit
						TCCR5A |= (1<<COM1A1);	
					}
				} else {
					if(curConfig[0].drvDir){		// test for CW/CCW
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR1A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
						ICR5 = 0x80;
						OCR5A = (value & 0x7f);	// low 7 bit
						TCCR5A |= (1<<COM1A1);
					} else {
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR5A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
					
						ICR1 = 0x80;
						OCR1A = (value & 0x7f);	// low 7 bit
						TCCR1A |= (1<<COM1A1);
					}
				}
			break;
		case DRIVE1:
				M1RSTPORT |= (1<<M1RST);
				M1APORT |= (1<<M1AHI);
				M1BPORT |= (1<<M1BHI);
			
				if(CHECKBIT(value,7)){
					if(curConfig[1].drvDir){
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);					
						TCCR4A &= ~(1<<COM1A1);
						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR3A = (value & 0x7f);	// low 7 bit
						ICR3 = 0x80;
						TCCR3A |= (1<<COM1A1);							
					} else {
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);
						TCCR3A &= ~(1<<COM1A1);

						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR4A = (value & 0x7f);	// low 7 bit
						ICR4 = 0x80;
						TCCR4A |= (1<<COM1A1);	
					}
				} else {
					if(curConfig[1].drvDir){
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);
						TCCR3A &= ~(1<<COM1A1);

						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR4A = (value & 0x7f);	// low 7 bit
						ICR4 = 0x80;
						TCCR4A |= (1<<COM1A1);	
					} else {
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);					
						TCCR4A &= ~(1<<COM1A1);
						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR3A = (value & 0x7f);	// low 7 bit
						ICR3 = 0x80;
						TCCR3A |= (1<<COM1A1);	
					}
				}
			break;
		case ALL_DRIVE:
				// drive 1
				M0RSTPORT |= (1<<M0RST);
				M0APORT |= (1<<M0AHI);
				M0BPORT |= (1<<M0BHI);
				
				if(CHECKBIT(value,7)) {
					if(curConfig[0].drvDir){		// test for CW/CCW
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR5A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
					
						ICR1 = 0x80;
						OCR1A = (value & 0x7f);	// low 7 bit
						TCCR1A |= (1<<COM1A1);	
					} else {
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR1A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
						ICR5 = 0x80;
						OCR5A = (value & 0x7f);	// low 7 bit
						TCCR5A |= (1<<COM1A1);	
					}
				} else {
					if(curConfig[0].drvDir){		// test for CW/CCW
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR1A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
						ICR5 = 0x80;
						OCR5A = (value & 0x7f);	// low 7 bit
						TCCR5A |= (1<<COM1A1);
					} else {
						M0APORT &=~(1<<M0ALO);
						M0BPORT &=~(1<<M0BLO);					
						TCCR5A &= ~(1<<COM1A1);
					
						TCNT5 = 0;
						TCNT1 = 0;
					
						ICR1 = 0x80;
						OCR1A = (value & 0x7f);	// low 7 bit
						TCCR1A |= (1<<COM1A1);
					}
				}
				
				// drive 2
				M1RSTPORT |= (1<<M1RST);
				M1APORT |= (1<<M1AHI);
				M1BPORT |= (1<<M1BHI);
			
				if(CHECKBIT(value,7)){
					if(curConfig[1].drvDir){
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);					
						TCCR4A &= ~(1<<COM1A1);
						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR3A = (value & 0x7f);	// low 7 bit
						ICR3 = 0x80;
						TCCR3A |= (1<<COM1A1);							
					} else {
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);
						TCCR3A &= ~(1<<COM1A1);

						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR4A = (value & 0x7f);	// low 7 bit
						ICR4 = 0x80;
						TCCR4A |= (1<<COM1A1);	
					}
				} else {
					if(curConfig[1].drvDir){
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);
						TCCR3A &= ~(1<<COM1A1);

						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR4A = (value & 0x7f);	// low 7 bit
						ICR4 = 0x80;
						TCCR4A |= (1<<COM1A1);	
					} else {
						M1APORT &=~(1<<M1ALO);
						M1BPORT &=~(1<<M1BLO);					
						TCCR4A &= ~(1<<COM1A1);
						TCNT3 = 0;
						TCNT4 = 0;
					
						OCR3A = (value & 0x7f);	// low 7 bit
						ICR3 = 0x80;
						TCCR3A |= (1<<COM1A1);	
					}
				}
			break;
		default: break;
	}
}

void DriveRst(uint8_t idx)
{
	switch(idx){
		case DRIVE0:
			TCCR5A &= ~(1<<COM1A1);
			TCCR1A &= ~(1<<COM1A1);
			
			M0RSTPORT &= ~(1<<M0RST);
			break;
		case DRIVE1:
			TCCR3A &= ~(1<<COM1A1);
			TCCR4A &= ~(1<<COM1A1);
			
			M1RSTPORT &= ~(1<<M1RST);
			break;
		case ALL_DRIVE:
			TCCR5A &= ~(1<<COM1A1);
			TCCR1A &= ~(1<<COM1A1);
			
			TCCR3A &= ~(1<<COM1A1);
			TCCR4A &= ~(1<<COM1A1);
			
			M0RSTPORT &= ~(1<<M0RST);
			M1RSTPORT &= ~(1<<M1RST);
			break;
	}
}

void DriveWind(uint8_t idx)
{
	switch(idx){
		case DRIVE0:
			TCCR5A &= ~(1<<COM1A1);
			TCCR1A &= ~(1<<COM1A1);
			M0RSTPORT |= (1<<M0RST);
			M0APORT |= (1<<M0AHI);
			M0BPORT |= (1<<M0BHI);
			M0APORT |= (1<<M0ALO);
			M0BPORT |= (1<<M0BLO);
		break;
		case DRIVE1:
			TCCR3A &= ~(1<<COM1A1);
			TCCR4A &= ~(1<<COM1A1);
			M1RSTPORT |= (1<<M1RST);
			M1APORT |= (1<<M1AHI);
			M1BPORT |= (1<<M1BHI);
			M1APORT |= (1<<M1ALO);
			M1BPORT |= (1<<M1BLO);
		break;
		case ALL_DRIVE:
			TCCR5A &= ~(1<<COM1A1);
			TCCR1A &= ~(1<<COM1A1);
			
			TCCR3A &= ~(1<<COM1A1);
			TCCR4A &= ~(1<<COM1A1);
			
			M0RSTPORT |= (1<<M0RST);
			M0APORT |= (1<<M0AHI);
			M0BPORT |= (1<<M0BHI);
			M0APORT |= (1<<M0ALO);
			M0BPORT |= (1<<M0BLO);
			
			M1RSTPORT |= (1<<M1RST);
			M1APORT |= (1<<M1AHI);
			M1BPORT |= (1<<M1BHI);
			M1APORT |= (1<<M1ALO);
			M1BPORT |= (1<<M1BLO);
		break;
	}
}
